import lejos.nxt.*;
public class LightFollower  {
	
	private static int averageLeft, averageRight;
	private static int leftValue, rightValue;
	private static double beta = 0.9;
	
	
		
	public static void main(String[] args) throws Exception {
		
		LightSensor l1 = new LightSensor(SensorPort.S1);
		LightSensor l2 = new LightSensor(SensorPort.S2);
		
		while(!Button.ESCAPE.isPressed()) {
		
			leftValue = SensorPort.S1.readRawValue();
			rightValue = SensorPort.S2.readRawValue();
			MotorPort.A.controlMotor(100, 1);
			
			averageLeft = (int) ((beta * normalize(leftValue)) + ((1-beta)*averageLeft));
			averageRight = (int) ((beta * normalize(rightValue)) + ((1-beta)*averageRight));
			
			LCD.drawInt(averageLeft, 1, 0);
			LCD.drawInt(averageRight, 1, 1);
			
			LCD.drawInt(normalize(leftValue), 1, 3);
			LCD.drawInt(normalize(rightValue), 1, 4);
			
			if(normalize(leftValue) > averageLeft) {
				MotorPort.B.controlMotor(normalize(rightValue), 1);
			} else {
				MotorPort.B.controlMotor(0, 3);
			}

			if(normalize(rightValue) > averageRight) {
				MotorPort.C.controlMotor(normalize(leftValue), 1);
			} else {
				MotorPort.C.controlMotor(0, 3);
			}
			
			
				
		}
		
	
	}
	private static int normalize(int light) {
		int MAX_LIGHT = 100;
		int MIN_LIGHT = 800;
		int output = (100 - ((light - MAX_LIGHT) * 100) / (MIN_LIGHT - MAX_LIGHT));
		
		if(output < 0) 
			return 0;
		if(output > 100) 
			return 100;
		
		return output;
	}
	
	private static void run1()
	{
	}
	
	private static void run2()
	{
	}
	
	private class threadClass implements Thread
	{
		
	}
}


